#!/usr/bin/env python3

import sys, time
import threading
import binascii
import json
import struct
import pigpio

from multiprocessing import Process,Queue

class Servo(object):
    """docstring for Servo"""
    local_url = "ipc:///tmp/servo.ipc"
    web_url = "ipc:///tmp/web.ipc"
    drive_url = "ipc:///tmp/drive.ipc"

    remote_urls = [web_url, drive_url]

    # pin gpio number
    VERT_PIN = 21
    HORI_PIN = 20

    # servo pwm frequence 50HZ
    SERVO_PWM_HZ = 50

    DEVICE_NAME = "servo"

    # record servo moving state, stop state is 0
    TURN_VERT_UP = (1 << 0)
    TURN_VERT_DOWN = (1 << 1)
    TURN_HORI_LEFT = (1 << 2)
    TURN_HORI_RIGHT = (1 << 3)

    def __init__(self, queue):
        ''' init ipc socket, gpio and servo position state '''
        # dict for ipc call
        self.OPERAT_DICT = {
            "position_reset" : self.position_reset,
            "turn_left": self.turn_left,
            "turn_right": self.turn_right,
            "turn_up": self.turn_up,
            "turn_down": self.turn_down,
            "turn_stop": self.turn_stop,
        }

        self.queue = queue

        self.pi = pigpio.pi()

        if not self.pi.connected:
            print("servo init failed... Not working")

        self.gpio_init()

        # used for save servo position 
        self.hori = 90
        self.vert = 90
        self.stat = 0

        self.position_reset()

        # moving servo thread
        self.stat_mutex = threading.Lock()
        # self.daemon_move()

    def gpio_init(self):
        self.pi.set_PWM_range(self.HORI_PIN, 100)
        self.pi.set_PWM_range(self.VERT_PIN, 100)

        self.pi.set_PWM_frequency(self.HORI_PIN, self.SERVO_PWM_HZ)
        self.pi.set_PWM_dutycycle(self.HORI_PIN, 0)

        self.pi.set_PWM_frequency(self.VERT_PIN, self.SERVO_PWM_HZ)
        self.pi.set_PWM_dutycycle(self.VERT_PIN, 0)

    def turn_left(self):
        self.stat_mutex.acquire()
        self.stat |= self.TURN_HORI_LEFT 
        self.stat &= (~self.TURN_HORI_RIGHT)
        self.stat_mutex.release()

    def turn_right(self):
        self.stat_mutex.acquire()
        self.stat |= self.TURN_HORI_RIGHT 
        self.stat &= (~self.TURN_HORI_LEFT)
        self.stat_mutex.release()
        
    def turn_up(self):
        self.stat_mutex.acquire()
        self.stat |= self.TURN_VERT_UP
        self.stat &= (~self.TURN_VERT_DOWN)
        self.stat_mutex.release()
        
    def turn_down(self):
        self.stat_mutex.acquire()
        self.stat |= self.TURN_VERT_DOWN 
        self.stat &= (~self.TURN_VERT_UP)
        self.stat_mutex.release()

    def turn_stop(self):
        self.stat_mutex.acquire()
        self.stat = 0
        self.stat_mutex.release()

    def daemon_move(self):
        thread_move_servo = threading.Thread(target=self.proc_servo_stat)
        thread_move_servo.setDaemon(True)
        thread_move_servo.start()


    def proc_msg(self):
        while True:
            try:
                msg_bytes = self.queue.get(True)
                msg_str = msg_bytes.decode('utf-8')
                msg = json.loads(msg_str)

                print(type(msg["head"]["device"]), msg["head"]["device"])
                if msg["head"]["device"] == self.DEVICE_NAME:
                    self.msg_parse(msg)
            except:
                print("queue get error")

    def msg_parse(self, msg):
        method = msg["head"]["method"]
        print(method)
        if method in self.OPERAT_DICT:
            self.OPERAT_DICT[method]()

    # reset the servo position to fixed value
    def position_reset(self):
        self.move_hori(0)
        self.move_vert(90)
        time.sleep(1)


    def proc_servo_stat(self):
        while True:
            stat = 0
            stat_vert = False
            stat_hori = False

            self.stat_mutex.acquire()
            stat = self.stat
            self.stat_mutex.release()

            # get hori move state
            if (stat & self.TURN_HORI_LEFT):
                if (0 <= self.hori <= 150):
                    self.hori += 10
                    stat_hori = True
            if (stat & self.TURN_HORI_RIGHT):
                if (10 <= self.hori <= 180):
                    self.hori -= 10
                    stat_hori = True
            if stat_hori:
                self.move_hori(self.hori)

            # get vert move state
            if (stat & self.TURN_VERT_DOWN):
                if (0 <= self.vert <= 150):
                    self.vert += 10
                    stat_vert = True
            elif (stat & self.TURN_VERT_UP):
                if (10 <= self.vert <= 180):
                    self.vert -= 10
                    stat_vert = True
            if stat_vert:
                self.move_vert(self.vert)

            if stat == 0:
                self.move_stop()

            # time.sleep(0.2) 


    def move_vert(self, angle): 
        if (0 <= angle <= 180):
            self.pi.set_PWM_dutycycle(self.VERT_PIN, 2.5 + 10 * angle / 180)
            time.sleep(0.02)                                # wait 20ms  
            time.sleep(0.1)
        else:
            print("error vert angle: %d" %(angle))

    # angle: 0 ~ 180
    def move_hori(self, angle):
        if (0 <= angle <= 180):
            self.pi.set_PWM_dutycycle(self.HORI_PIN, 2.5 + 10 * angle / 180)
            time.sleep(0.02)                                # wait 20ms  
            time.sleep(0.1)
        else:
            print("error hori angle: %d" %(angle))

    def move_stop(self):
        self.pi.set_PWM_dutycycle(self.HORI_PIN, 0)
        self.pi.set_PWM_dutycycle(self.VERT_PIN, 0)

    def __del__(self):
        self.move_stop()
        self.pi.set_mode(self.HORI_PIN, pigpio.INPUT)
        self.pi.set_pull_up_down(self.HORI_PIN, pigpio.PUD_UP)
        self.pi.set_mode(self.VERT_PIN, pigpio.INPUT)
        self.pi.set_pull_up_down(self.VERT_PIN, pigpio.PUD_UP)     
        print("stop")   
        self.pi.stop()

    def test(self):
        cycle = 2
        for i in range(0, 181, 10):
            print(i)
            self.move_vert(i)
            self.move_hori(i)
            # time.sleep(0.2)
 

def main():
    RcvQueue = Queue(1027)

    servo = Servo(RcvQueue)
    servo.test()
    servo.__del__()

    # servo.position_reset()

if __name__ == '__main__':
    main()